#include "daq.h"
#include "util/util.h"
#include <stdio.h>
#include "../global.h"
#include <math.h>

using namespace DAQ_namespace;

extern DAQALLOC DAQ;

extern EVENT_HANDLE eh;
extern DWORD eventMsk;
extern DWORD eventWord;

double processedDataBuffer[7];

//extern pthread_t primary;  //this is the primary thread.  The program state machine exists here
//extern pthread_t receive;  //this thread handles and buffers incoming Rx data

//GENERAL MSGS

void DAQALLOC::startConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->state = DAQ_RUNNING;
    }
    this->general.startPending = false;
}
void DAQALLOC::stopConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->state = DAQ_STOPPED_STATE;
    }
    this->general.stopPending = false;
}

void DAQALLOC::readyConfig()
{

        this->state = DAQ_INIT_STATE;
}


//ANALOG IN

void DAQALLOC::analogInConfig(uint8_t errorCode)
{
    if      (errorCode & ADC_CH0_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[0].enable = false;
        }
        else
        {
            this->analogIn[0].enable = true;
        }

        this->analogIn[0].settingPending = false;

    }
    else if (errorCode & ADC_CH1_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[1].enable = false;
        }
        else
        {
            this->analogIn[1].enable = true;
        }

        this->analogIn[1].settingPending = false;

    }
    else if (errorCode & ADC_CH2_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[2].enable = false;
        }
        else
        {
            this->analogIn[2].enable = true;
        }

        this->analogIn[2].settingPending = false;

    }
    else if (errorCode & ADC_CH3_bp<<4)
    {
        //check to see if the ADC GAIN was turned off
        if( (errorCode & 0x7) == ADC_CH_GAIN_OFF_gc)
        {
            this->analogIn[3].enable = false;
        }
        else
        {
            this->analogIn[3].enable = true;
        }

        this->analogIn[3].settingPending = false;

    }
}

//ANALOG OUT

void DAQALLOC::analogOutConfig(uint8_t errorCode)
{
    if(errorCode & DAC_CH0_bp)
    {

        //if there is a way to turn it off add here

        this->analogOut[0].enable = true;
        this->analogOut[0].settingPending = false;
    }
    else if(errorCode & DAC_CH1_bp)
    {

        //if there is a way to turn it off add here

        this->analogOut[1].enable = true;
        this->analogOut[1].settingPending = false;
    }
}

//DIGITAL IN

void DAQALLOC::digitalInConfig(uint8_t errorCode)
{
    if (!(errorCode & 0x80))
    {
        this->digital.inSettingPending[errorCode] = false;
        this->digital.inEnable = FEATURE_UTILIZED;
        this->digital.dioAlloc[errorCode & 0xF] = FEATURE_UTILIZED;
        this->digital.src[errorCode & 0xF] = DIO_INPUT;
    }
}

//DIGITAL OUT

void DAQALLOC::digitalOutConfig(uint8_t errorCode)
{
    if (!(errorCode & 0x80))
    {
        this->digital.outSettingPending[errorCode] = false;
        this->digital.dioAlloc[errorCode & 0xF] = FEATURE_UTILIZED;
        this->digital.src[errorCode & 0xF] = DIO_OUTPUT;
    }
}

//ENCODER

//This function handles enc config message acks
void DAQALLOC::encoderConfig(uint8_t errorCode)
{
    if      (errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->encoder[0].enable = true;
            this->encoder[0].settingPending = false;

            encDioConfig(0);
        }
        else
        {
            this->encoder[1].enable = true;
            this->encoder[1].settingPending = false;

            encDioConfig(1);
        }

    }
    else if(errorCode & 0x1)
    {
        this->encoder[0].settingPending = false;

    }
    else if(errorCode & 0x2)
    {
        this->encoder[1].settingPending = false;
    }
}

//This function backward annotates encoder i/o allocations based on the PLOC and index settings
void DAQALLOC::encDioConfig(uint8_t encNum)
{
    switch(encoder[encNum].PLOC)
    {
    case PLOC_D1:

        this->digital.dioAlloc[1] = FEATURE_UTILIZED;
        this->digital.src[1] = DIO_ENC_CHA;
        this->digital.dioAlloc[2] = FEATURE_UTILIZED;
        this->digital.src[2] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[3] = FEATURE_UTILIZED;
            this->digital.src[3] = DIO_ENC_INDX;
        }

    break;
    case PLOC_D2:
        this->digital.dioAlloc[2] = FEATURE_UTILIZED;
        this->digital.src[2] = DIO_ENC_CHA;
        this->digital.dioAlloc[3] = FEATURE_UTILIZED;
        this->digital.src[3] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[4] = FEATURE_UTILIZED;
            this->digital.src[4] = DIO_ENC_INDX;
        }

    break;
    case PLOC_D3:

        this->digital.dioAlloc[3] = FEATURE_UTILIZED;
        this->digital.src[3] = DIO_ENC_CHA;
        this->digital.dioAlloc[4] = FEATURE_UTILIZED;
        this->digital.src[4] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[5] = FEATURE_UTILIZED;
            this->digital.src[5] = DIO_ENC_INDX;
        }

    break;
    case PLOC_D4:

        this->digital.dioAlloc[4] = FEATURE_UTILIZED;
        this->digital.src[4] = DIO_ENC_CHA;
        this->digital.dioAlloc[5] = FEATURE_UTILIZED;
        this->digital.src[5] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[6] = FEATURE_UTILIZED;
            this->digital.src[6] = DIO_ENC_INDX;
        }

    break;
    case PLOC_D5:

        this->digital.dioAlloc[5] = FEATURE_UTILIZED;
        this->digital.src[5] = DIO_ENC_CHA;
        this->digital.dioAlloc[6] = FEATURE_UTILIZED;
        this->digital.src[6] = DIO_ENC_CHB;

    break;
    case PLOC_D10:

        this->digital.dioAlloc[10] = FEATURE_UTILIZED;
        this->digital.src[10] = DIO_ENC_CHA;
        this->digital.dioAlloc[9] = FEATURE_UTILIZED;
        this->digital.src[9] = DIO_ENC_CHB;

    break;
    case PLOC_D11:

        this->digital.dioAlloc[11] = FEATURE_UTILIZED;
        this->digital.src[11] = DIO_ENC_CHA;
        this->digital.dioAlloc[10] = FEATURE_UTILIZED;
        this->digital.src[10] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[9] = FEATURE_UTILIZED;
            this->digital.src[9] = DIO_ENC_INDX;
        }

    break;
    case PLOC_D12:

        this->digital.dioAlloc[12] = FEATURE_UTILIZED;
        this->digital.src[12] = DIO_ENC_CHA;
        this->digital.dioAlloc[11] = FEATURE_UTILIZED;
        this->digital.src[11] = DIO_ENC_CHB;

        if(this->encoder[encNum].index)
        {
            this->digital.dioAlloc[10] = FEATURE_UTILIZED;
            this->digital.src[10] = DIO_ENC_INDX;
        }

    break;
    }
}

//MOTOR

void DAQALLOC::motorConfig(uint8_t errorCode)
{
    if(errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->motor[0].settingPending = false;
            this->motor[0].enable = true;

            this->digital.dioAlloc[1] = FEATURE_UTILIZED;
            this->digital.src[1] = DIO_PWM;
        }
        if(errorCode & 0x2)
        {
            this->motor[1].settingPending = false;
            this->motor[1].enable = true;

            this->digital.dioAlloc[2] = FEATURE_UTILIZED;
            this->digital.src[2] = DIO_PWM;
        }
        if(errorCode & 0x4)
        {
            this->motor[2].settingPending = false;
            this->motor[2].enable = true;

            this->digital.dioAlloc[3] = FEATURE_UTILIZED;
            this->digital.src[3] = DIO_PWM;
        }
        if(errorCode & 0x8)
        {
            this->motor[3].settingPending = false;
            this->motor[3].enable = true;

            this->digital.dioAlloc[4] = FEATURE_UTILIZED;
            this->digital.src[4] = DIO_PWM;
        }
    }
    else
    {
        if(errorCode & 0x1) this->motor[0].settingPending = false;
        if(errorCode & 0x2) this->motor[1].settingPending = false;
        if(errorCode & 0x4) this->motor[2].settingPending = false;
        if(errorCode & 0x8) this->motor[3].settingPending = false;
    }
}

//SERVO

void DAQALLOC::servoConfig(uint8_t errorCode)
{
    if(errorCode & 0x80)
    {
        if(errorCode & 0x1)
        {
            this->servo[0].settingPending = false;
            this->servo[0].enable = true;

            this->digital.dioAlloc[12] = FEATURE_UTILIZED;
            this->digital.src[12] = DIO_PWM;
        }
        if(errorCode & 0x2)
        {
            this->servo[1].settingPending = false;
            this->servo[1].enable = true;

            this->digital.dioAlloc[11] = FEATURE_UTILIZED;
            this->digital.src[11] = DIO_PWM;
        }
        if(errorCode & 0x4)
        {
            this->servo[2].settingPending = false;
            this->servo[2].enable = true;

            this->digital.dioAlloc[10] = FEATURE_UTILIZED;
            this->digital.src[10] = DIO_PWM;
        }
        if(errorCode & 0x8)
        {
            this->servo[3].settingPending = false;
            this->servo[3].enable = true;

            this->digital.dioAlloc[9] = FEATURE_UTILIZED;
            this->digital.src[9] = DIO_PWM;
        }
    }
    else
    {
        if(errorCode & 0x1) this->servo[0].settingPending = false;
        if(errorCode & 0x2) this->servo[1].settingPending = false;
        if(errorCode & 0x4) this->servo[2].settingPending = false;
        if(errorCode & 0x8) this->servo[3].settingPending = false;
    }
}

//TIMER

void DAQALLOC::tmrConfig(uint8_t errorCode)
{
    if(errorCode == 1)
    {
        this->timer.enable = true;
    }
    this->timer.settingPending = false;
}


//SUPER PACKET HANDLER
void DAQALLOC::inputSuperPacketHandler(uint8_t packet[])
{
    WORD_t val;
    LONG_WORD_t val32;

    uint8_t pos = 2;
    uint8_t inc;

    //ANALOG
    for(inc=0;inc<4;inc++)
    {
        if(this->analogIn[inc].enable)
        {
            val.bytes.byteH = packet[pos++];
            val.bytes.byteL = packet[pos++];
            processedDataBuffer[inc] = itoV(this->analogIn[inc].gain,val.word);
        }
        else processedDataBuffer[inc] = 0;
    }
	
    for(inc=0;inc<2;inc++)
    {
        if(this->encoder[inc].enable)
        {
            if(this->encoder[inc].absolute)
            {
                val.bytes.byteH = packet[pos++];
                val.bytes.byteL = packet[pos++];
                processedDataBuffer[4+inc] = val.word;
            }
            else
            {
                val32.bytes.byteHH = packet[pos++];
                val32.bytes.byteMH = packet[pos++];
                val32.bytes.byteML = packet[pos++];
                val32.bytes.byteLL = packet[pos++];
                processedDataBuffer[4+inc] = val32.long_word;
            }
        }
        else processedDataBuffer[4+inc] = 0;
    }

    if(this->digital.inEnable)
    {
        val.bytes.byteH = packet[pos++];
        val.bytes.byteL = packet[pos++];

        processedDataBuffer[6] = (val.word & 0x1FFF);
    }
    else processedDataBuffer[6] = 0;


}


double itoV(GAIN_ENUM gain, int16_t conversion)
{
    switch(gain)
    {
        case(ADC_CH_GAIN_1X_gc):
            return 10.0267*(conversion)/8191.0;
        break;
        case(ADC_CH_GAIN_2X_gc):
            return 5.0134*(conversion)/8191.0;
        break;
        case(ADC_CH_GAIN_4X_gc):
            return 2.5067*(conversion)/8191.0;
        break;
        case(ADC_CH_GAIN_8X_gc):
            return 1.2533*(conversion)/8191.0;
        break;
        case(ADC_CH_GAIN_16X_gc):
            return 0.62667*(conversion)/8191.0;
        break;
        case(ADC_CH_GAIN_32X_gc):
            return 0.31334*(conversion)/8191.0;
        break;
    }
    //catch all
    return 0;
}
